#ifndef __ROBOT_H__
#define __ROBOT_H__

#include <stdio.h>
#include "SE3Math.h"

typedef struct{
    float joint1_limit_min; 
    float joint1_limit_max;
    float joint2_limit_min; 
    float joint2_limit_max;
    float joint3_limit_min; 
    float joint3_limit_max;
    float joint4_limit_min; 
    float joint4_limit_max;
    float joint5_limit_min; 
    float joint5_limit_max;
    float joint6_limit_min; 
    float joint6_limit_max;
} JointsLimits;

typedef struct{
    int dof;
    DH dh_0_1;
    DH dh_1_2;
    DH dh_2_3;
    DH dh_3_4;
    DH dh_4_5;
    DH dh_5_6;
    DH dh_end_tcp;
    DH dh_base_0;
} Robot;

typedef struct{
    float theta1;
    float theta2;
    float theta3;
    float theta4;
    float theta5;
    float theta6;
} Joints;

typedef struct{
    Joints ik_results[8];
} PossibleIK;


void robot_init();
void setRobotModel(DH* dh_list, int dof);
void setRobotTcp(DH* dh);
void setRobotBase(DH* dh);
void getRobotTfTree(Joints* joints, Matrix* tf_tree);
void forward_kinematics(Joints* joints, Pose* pose);
void inverse_kinematics(Pose* target_pose, PossibleIK* ik_joints);
int compute_best_ik(PossibleIK* ik_joints, Joints* start_joints, Joints* best_ik);
void get_best_ik(Pose* target_pose, Joints* start_joints, Joints* best_ik);
int jointSpacePlanner(Joints* start_joints, Joints* end_joints, Joints* mid_joints_list, int max_num);

#endif